package example.com.nexd.collectionsdk.sdk.utils;


public class StepDetector {

    ButtLPFilter filter = new ButtLPFilter();

    final int THL = 4;
    final float THH = 0.11f;

    int st = 1;
    int ct = 0;
    int pt = 1;
    float xst = 0;
    float xpt = 0;
    int flag = 0;
    int trend = 0;
    int index = 0;
    float lastValue = 0;

    private int stepCount = 0;

    public int getStepCount() {
        return stepCount;
    }

    public StepDetector() {
        clear();
    }

    public void clear() {
        st = 1;
        ct = 0;
        pt = 1;
        xst = 0;
        xpt = 0;
        flag = 0;
        trend = 0;
        index = 0;
        lastValue = 0;

        stepCount = 0;

        filter.clear();
    }

    public int detect(float input) {

        // LP Filter
        input = filter.filter(input);

        index = index + 1;
        if (index == 1) {
            trend = (int) Math.signum(input - lastValue);
            lastValue = input;
            return 0;
        }
        int tt = (int) Math.signum(input - lastValue);
        if (tt == trend) {
            lastValue = input;
            return 0;
        }
        trend = tt;
        if (flag == 0) {
            st = pt;
            pt = index;
            xst = xpt;
            xpt = input;
        } else {
            pt = index;
            flag = 0;
            xpt = input;
        }
        if (pt - st < THL || Math.abs(xpt - xst) < THH) {
            flag = 1;
            lastValue = input;
            return 0;
        }

        if (ct == 2) {
            ct = 1;
            lastValue = input;
            stepCount++;
            return 1;
        } else {
            ct = ct + 1;
        }
        lastValue = input;
        return 0;
    }

    public class ButtLPFilter {
        final static int BUFFER_LENGTH = 20;
        double[] xbuffer = new double[BUFFER_LENGTH];
        double[] ybuffer = new double[BUFFER_LENGTH];
        /*
         * PARAM 3
		double[] B_Arr = {	1.000000000000000,
				   -2.788055939605452,
				    3.046470778186613,
				   -1.523350025713034,
				    0.292289551535585};
		double[] A_Arr = {  0.001709647775232,
			    0.006838591100928,
			    0.010257886651392,
			    0.006838591100928,
			    0.001709647775232};
		*/

        // * PARAM 1
//		double[] A_Arr = {0.000298940539714,
//				0.001793643238285,   
//				0.004484108095713,   
//				0.005978810794282,
//				0.004484108095712,   
//				0.001793643238285,   
//				0.000298940539714
//				};
//		double[] B_Arr = {1.000000000000000,  
//				   -3.639852443812422,
//				    5.828361659135576,  
//				   -5.169523378660825,
//				    2.657879849024126,  
//				   -0.747097954410503,   
//				    0.089364463265755};
//		

		/*
         * PARAM 2
		double[] A_Arr = {0.001150742544106,
				0.005753712720530,
				0.011507425441061,   
				0.011507425441061,
				0.005753712720530,
				0.001150742544106

				};
		double[] B_Arr = {		1.000000000000000,
				   -3.025837663462862,
				    3.917198222080807,  
				   -2.645651055885413,
				    0.923532450543623,
				   -0.132418191864761};
				   */

        //		double[] B_Arr={1,-11.0222097446940,56.6665859844020,-180.066544205202,395.020334279893,-632.741536869534,763.028117458715,-703.608035215701,498.479689323329,-269.969729510025,110.011600084360,-32.7040806407390,6.70397775709147,-0.848123531717302,0.0499548833483934};
//		double[] A_Arr = {3.26702553898883e-12,4.57383575458437e-11,2.97299324047984e-10,1.18919729619194e-09,3.27029256452782e-09,6.54058512905564e-09,9.81087769358346e-09,1.12124316498097e-08,9.81087769358346e-09,6.54058512905564e-09,3.27029256452782e-09,1.18919729619194e-09,2.97299324047984e-10,4.57383575458437e-11,3.26702553898883e-12};
//		double[] A_Arr = {2.24334439913321e-13,3.36501659869981e-12,2.35551161908987e-11,1.02072170160561e-10,3.06216510481683e-10,6.73676323059702e-10,1.12279387176617e-09,1.44359212084222e-09,1.44359212084222e-09,1.12279387176617e-09,6.73676323059702e-10,3.06216510481683e-10,1.02072170160561e-10,2.35551161908987e-11,3.36501659869981e-12,2.24334439913321e-13};
//		double[] B_Arr = {1,-11.9891091427451,67.3300948679838,-234.915568267038,569.381652123658,-1015.37296113253,1376.08926902244,-1443.05902815935,1180.45513742372,-753.178101390691,371.729248358900,-139.356584941186,38.4094636308401,-7.34719906065887,0.872107810850588,-0.0484211368422843};
        double A_Arr[] = {
                2.24334439913321E-013D, 3.36501659869981E-012D, 2.3555116190898699E-011D, 1.0207217016056101E-010D, 3.0621651048168301E-010D, 6.7367632305970204E-010D, 1.1227938717661701E-009D, 1.4435921208422199E-009D, 1.4435921208422199E-009D, 1.1227938717661701E-009D,
                6.7367632305970204E-010D, 3.0621651048168301E-010D, 1.0207217016056101E-010D, 2.3555116190898699E-011D, 3.36501659869981E-012D, 2.24334439913321E-013D
        };
        double B_Arr[] = {
                1.0D, -11.9891091427451D, 67.330094867983803D, -234.915568267038D, 569.381652123658D, -1015.37296113253D, 1376.08926902244D, -1443.05902815935D, 1180.4551374237201D, -753.17810139069104D,
                371.72924835890001D, -139.35658494118599D, 38.409463630840101D, -7.3471990606588697D, 0.87210781085058797D, -0.048421136842284301D
        };

        int count = 0;

        public ButtLPFilter() {
            clear();
        }


        //  clear the double array with default value
        public void clear() {
            count = 0;
            for (int i = 0; i < BUFFER_LENGTH; i++) {
                xbuffer[i] = 0;
                ybuffer[i] = 0;
            }
        }


        // exchange the behind value to forward
        private void shift(double[] arr) {
            for (int i = 0; i < arr.length - 1; i++) {
                arr[i] = arr[i + 1];
            }
        }

        // filter the sensor y-value
        public float filter(float input) {
            double w = 0, v = 0;
            int k;
            shift(xbuffer);
            shift(ybuffer);
            xbuffer[xbuffer.length - 1] = input;
            count++;
            for (k = 0; k < A_Arr.length; k++) {
                    if (count - k > 0) {
                    v = (double) (v + A_Arr[k] * (double) xbuffer[xbuffer.length - 1 - k]);
                }
            }
            for (k = 1; k < B_Arr.length; k++) {
                if (count - k > 0) {
                    w = (double) (w + B_Arr[k] * (double) ybuffer[ybuffer.length - 1 - k]);
                }
            }
            ybuffer[ybuffer.length - 1] = (double) ((-w + v) / B_Arr[0]);
            return (float) ybuffer[ybuffer.length - 1];
        }
    }
}